#include <iostream>
#include <cmath>

#include "system.h"

System::System(unsigned int _totalModes) : currentMode(0), totalModes(_totalModes){
	for (unsigned int i = 0; i < _totalModes; i++){
		alphas.push_back(0.0);
		gammas.push_back(0.0);
	}
}

System::~System(){
	delete lambda;
	delete x;
}

void System::setAlpha(double _alpha){
	alphas[currentMode] = _alpha;
}

void System::setGamma(double _gamma){
	gammas[currentMode] = _gamma;
}

double System::getAlpha(unsigned int mode){
	return alphas[mode];
}

double System::getGamma(unsigned int mode){
	return gammas[mode];
}

void System::setMode(unsigned int _m){
	currentMode = _m;
}

unsigned int System::getMode(){
	return currentMode;
}

unsigned int System::getTotalModes(){
	return totalModes;
}

void System::calculeVibrationMode(double error){
	Matrix k(getTotalModes(), getTotalModes(), NORMAL_MATRIX);
	Matrix m(getTotalModes(), getTotalModes(), IDENTITY_MATRIX);
	populateK(k);
	std::cout << "criada matriz K" << std::endl;
	populateM(m);
	std::cout << "criada matriz M" << std::endl;
	
	Matrix a = m.diagonalInverse() * k;
	
	QR method(a, error);
	method.calculate();
	std::cout << "feito QR" << std::endl;
	
	lambda = new Matrix(method.getLambda());
	x = new Matrix(method.getVector());
	
	sort(0, x->getColumn());
	std::cout << "feito sort" << std::endl;
	
	std::cout << "\n\nAuto valores" << std::endl;
	lambda->print();
	
	std::cout << "\n\nAuto vetores" << std::endl;
	x->print();
}

double System::getU(unsigned int mass, unsigned int mode){
	return (*x)[mass][(int) mode];
}

double System::getW(unsigned int mode){
	return sqrt((*lambda)[mode][(int) mode]);
}

void System::sort(int p, int q){//mergesort
	if (p<q-1){
		int m=(p+q)/2;
		this->sort(p,m);
		this->sort(m,q);
		
		int i=p, j=m, k=0;
		double w1[q-p];//quarda os lambdas
		Matrix w2(x->getRow(), q-p, NORMAL_MATRIX);//quarda os modos
		
		while (i<m && j<q){
			if ((*lambda)[i][i] < (*lambda)[j][j]){
				w1[k] = (*lambda)[i][i];
				for (int l = 0; l < (int) x->getRow(); l++){
					w2[l][k] = getU(l, i);
				}
				k++;
				i++;
			}
			else{
				w1[k] = (*lambda)[j][j];
				for (int l = 0; l < (int) x->getRow(); l++){
					w2[l][k] = getU(l, j);
				}
				k++;
				j++;
			}
		}
		while (i<m){
			w1[k] = (*lambda)[i][i];
			for (int l = 0; l < (int) x->getRow(); l++){
				w2[l][k] = getU(l, i);
			}
			k++;
			i++;
		}
		while (j<q){
			w1[k] = (*lambda)[j][j];
			for (int l = 0; l < (int) x->getRow(); l++){
				w2[l][k] = getU(l, j);
			}
			k++;
			j++;
		}
		
		for (int l = p; l < q; l++){
			(*lambda)[l][l] = w1[l-p];
			for (int l2 = 0; l2 < (int) x->getRow(); l2++){
				(*x)[l2][l] = w2[l2][l-p];
			}
		}
	}
	
}